#include <stdio.h>
#include <vector>
#include <iostream>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <math.h>
#include "color_point.h"
#include "PoseEstimation.h"



//using namespace std;

const int MIN_HUE = 100;
const int MAX_HUE = 130;
const int MIN_SAT = 75;
const int MAX_SAT = 256;
const int MIN_LIG = 60;
const int MAX_LIG = 255;


const int MIN_ELLIPSE_SIZE = 5;
const int MAX_ELLIPSE_SIZE = 30;


void color_point(IplImage* img, IplImage* blue_img, std::vector<Point> &blue_points)
{
	CvMemStorage* memStorage_blob = cvCreateMemStorage();
	CvSeq* contours_blob;		//all found contours
	
	CvScalar color_contour = CV_RGB(0,255,0);
	CvScalar color_ellipse = CV_RGB(255,0,0);

	//create an images to convert the BGR Image into a HSV-Image and to extract the hue channel
	IplImage* hsv_img;					//create a hsv image
	IplImage* hue_img;					//create a image for the hue channel of the hsv image
	IplImage* sat_img;					//create a image for the saturation channel of the hsv image
	IplImage* lig_img;					//create a image for the lightning channel of the hsv image
	IplImage* contour_img;				//image to determine contours

	hsv_img = cvCreateImage(cvSize(img->width, img->height), IPL_DEPTH_8U, 3);
	hue_img = cvCreateImage(cvSize(img->width, img->height), IPL_DEPTH_8U, 1);
	sat_img = cvCreateImage(cvSize(img->width, img->height), IPL_DEPTH_8U, 1);
	lig_img = cvCreateImage(cvSize(img->width, img->height), IPL_DEPTH_8U, 1);


//create an image that shows only blue color
//vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
	//converting in hsv-space
		cvCvtColor(img, hsv_img, CV_BGR2HSV);
		//cvNamedWindow("hsv_image",1);
		//cvShowImage("hsv_image",hsv_img);

		//extracting the hue channel
		cvSplit(hsv_img, hue_img, sat_img, lig_img, NULL);

	//treshold to extract the blue color (blue value in hsv = 120)
		
		//if src(x; y) > threshold: 0, otherwise: src(x; y)
		cvThreshold(hue_img, hue_img, MAX_HUE, 255, CV_THRESH_TOZERO_INV);
		//if src(x; y) > threshold: src, otherwise: 0
		cvThreshold(hue_img, hue_img, MIN_HUE, 255, CV_THRESH_BINARY);
		//cvNamedWindow("hue_image",1);
		//cvShowImage("hue_image",hue_img);
	
	//treshold to filter out the white color (filter out small saturation)
		cvThreshold(sat_img, sat_img, MAX_SAT, 255, CV_THRESH_TOZERO_INV);
		cvThreshold(sat_img, sat_img, MIN_SAT, 255, CV_THRESH_BINARY);

		cvThreshold(lig_img, lig_img, MAX_LIG, 255, CV_THRESH_TOZERO_INV);
		cvThreshold(lig_img, lig_img, MIN_LIG, 255, CV_THRESH_BINARY);

		//cvNamedWindow("sat_image",1);
		//cvShowImage("sat_image", sat_img);

		//cvThreshold(lig_img, lig_img, -1, 255, CV_THRESH_BINARY);
		//cvNamedWindow("lig_image",1);
		//cvShowImage("lig_image",lig_img);
		
		//combine the two channels
		cvAnd(hue_img, sat_img, blue_img, NULL);
		cvAnd(blue_img, lig_img, blue_img, NULL);

// end create an image that shows only blue color
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

	//create a contour image, find the contours and show them in an image
		contour_img = cvCloneImage(blue_img);
		//find contours in the image -> this function changes the image where the contours are searched
		int contours = cvFindContours(contour_img, memStorage_blob, & contours_blob, sizeof(CvContour),CV_RETR_EXTERNAL);
		//show the contours in an image
		//cvDrawContours( img, contours_blob, color_contour, color_contour, 2);
		//cvNamedWindow("Contours-Window",1);
		//cvShowImage("Contours-Window",img);

	int update = 0;
	for( ; contours_blob != 0; contours_blob = contours_blob->h_next )
	{
		//fit an ellipse around the blob
		CvBox2D ellipse;
		if ( contours_blob->total >= 6 )
			ellipse = cvFitEllipse2(contours_blob);
		else
			continue;

		//if ( (bounding_rect.height > MIN_ELLIPSE_SIZE) && (bounding_rect.width > MIN_ELLIPSE_SIZE) && (bounding_rect.height < MAX_ELLIPSE_SIZE) && (bounding_rect.width < MAX_ELLIPSE_SIZE) )
		if ( (ellipse.size.width > MIN_ELLIPSE_SIZE) && (ellipse.size.height > MIN_ELLIPSE_SIZE) && (ellipse.size.width < MAX_ELLIPSE_SIZE) && (ellipse.size.height < MAX_ELLIPSE_SIZE) 
			&& (ellipse.size.width/ellipse.size.height)<2 && (ellipse.size.height/ellipse.size.width)>0.5 )
		{
			//convert the parameters
			CvPoint center_point = cvPointFrom32f(ellipse.center);
			CvSize axis;
			axis.width = (int)((ellipse.size.width +0.5)/2);
			axis.height = (int)((ellipse.size.height +0.5)/2);

			//draw the ellipse:
				//cvEllipse(img, center_point, axis, ellipse.angle, 0, 360, color_ellipse);
			//draw the contours:
				//cvDrawContours( img, contours_blob, color_contour, color_contour, 2);
			
			//cvNamedWindow("Contours-Window",1);
			//cvShowImage("Contours-Window",img);
			//print the center point of the ellipse

			//cout << ellipse.center.x << " " << ellipse.center.y << endl;

			Point p;
			p.x = ellipse.center.x;
			p.y = ellipse.center.y;

			blue_points.push_back(p);

			//cvDrawContours( img, contours_blob, color_contour, color_contour, 0);

		}//end if condition for interesting ellipses
	}//end for loop contours

	/*
	int size = blue_points.size();
	for (int i=0;i<size;i++)
		cout << "point x/y " << blue_points[i] << endl;
	*/

	//release Memory
	cvReleaseImage(&hsv_img);
	cvReleaseImage(&hue_img);
	cvReleaseImage(&sat_img);
	cvReleaseImage(&lig_img);
	cvReleaseImage(&contour_img);
	cvReleaseMemStorage(&memStorage_blob);
}